// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PicknPlaceCylinderGrip.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.PicknPlace
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.PhysicalObjects;

    /// <summary>
    /// Implements IObjectGrip for cylindrical objects such as a can
    /// </summary>
    public class PicknPlaceCylinderGrip : PicknPlaceObjectGrip
    {
        /// <summary>
        /// Factor to offset palm from top of object in top down grip
        /// </summary>
        private readonly double topDownHeightExtraFactor = 1.3;
        
        /// <summary>
        /// Heuristic inclination of wrist angle for wrap around grips 5'
        /// </summary>
        private double wrapAroundHeuristicgripAngle = 5.0 / 180 * Math.PI;

        /// <summary>
        /// Cans are rotationally symmetric so can ignore the orientation supplied for now
        /// </summary>
        private Quaternion defaultOrientation = new Quaternion(0, 0, 0, 1);

        /// <summary>
        ///  Initializes a new instance of the <see cref="PicknPlaceCylinderGrip" /> class
        /// </summary>
        /// <param name="manpulatedObject">The object description</param>
        public PicknPlaceCylinderGrip(PhysicalObjectDescription manpulatedObject)
            : base(manpulatedObject)
        {
            this.SupportedGripStyles = new List<GripStyle>() 
                        {
                            GripStyle.WrapAroundPrincipalAxisFace1, 
                            GripStyle.TopDownAroundPrincipalAxis
                        };
        }

        /// <summary>
        /// Returns an enumerator for the collection
        /// </summary>
        /// <returns>Enumerator for the collection</returns>
        public override IEnumerator<GripStyle> GetEnumerator()
        {
            this.Reset();
            return this;
        }

        /// <summary>
        /// Returns the palm plane 
        /// </summary>
        /// <param name="gripStyle">The grip style</param>
        /// <param name="objectPose">Object's pose</param>
        /// <returns>plane normal direction and distance from origin</returns>
        public override Vector4 GetPalmPlane(GripStyle gripStyle, Pose objectPose)
        {
            switch (gripStyle)
            {
                case GripStyle.WrapAroundPrincipalAxisFace1:
                    return this.GetWrapAroundPalmPlane(objectPose);

                case GripStyle.TopDownAroundPrincipalAxis:
                    return this.GetTopDownPalmPlane(objectPose);

                default:
                    throw new ArgumentException(string.Format("Unknown cylindrical internalGrip type {0}", gripStyle.ToString()));
            }
        }

        /// <summary>
        /// Get the grip axis - the axis parallel to the palm plane running between thumb and fingers
        /// around which grip closes. For example with cylinder wraparound grip it is parallel to the 
        /// height axis.
        /// </summary>
        /// <param name="gripType">The grip type</param>
        /// <param name="objectPose">Object's pose in global coordinates</param>
        /// <returns>The grip axis</returns>
        public override Vector3 GetGripAxis(GripStyle gripType, Pose objectPose)
        {
            Vector3 gripAxis;

            switch (gripType)
            {
                case GripStyle.WrapAroundPrincipalAxisFace1:
                    Quaternion rot = this.HeuristicGripPlaneRotationForWrapAround(GripStyle.WrapAroundPrincipalAxisFace1, objectPose);
                    rot = rot * this.defaultOrientation;
                    gripAxis = Quaternion.Rotate(rot, new Vector3(0, 0, 1));
                    break;

                case GripStyle.TopDownAroundPrincipalAxis:
                    gripAxis = Quaternion.Rotate(this.defaultOrientation, Vector3.Normalize(new Vector3(objectPose.Position.X, objectPose.Position.Y, 0)));
                    break;

                default:
                    throw new ArgumentException(string.Format("Unknown cylindrical internalGrip type {0}", gripType.ToString()));
            }

            return gripAxis;
        }

        /// <summary>
        /// Gets the centre point of the grip along the axis
        /// </summary>
        /// <param name="gripType">The grip type</param>
        /// <param name="objectPose">Object's pose</param>
        /// <returns>Center grip point</returns>
        public override Vector3 GetGripCenter(GripStyle gripType, Pose objectPose)
        {
            Vector3 gripRot;

            switch (gripType)
            {
                case GripStyle.WrapAroundPrincipalAxisFace1:
                    Vector3 o = Vector3.Normalize(new Vector3(-objectPose.Position.X, -objectPose.Position.Y, 0.0)) * this.ManipulatedObjectDescription.WidthInM / 2.0;
                    gripRot = Quaternion.Rotate(this.defaultOrientation, new Vector3(o.X, o.Y, 0.0));
                    break;

                case GripStyle.TopDownAroundPrincipalAxis:
                    gripRot = Quaternion.Rotate(this.defaultOrientation, new Vector3(0, 0, this.ManipulatedObjectDescription.HeightInM / 2 * this.topDownHeightExtraFactor));
                    break;

                default:
                    throw new ArgumentException(string.Format("Unknown cylindrical internalGrip type {0}", gripType.ToString()));
            }

            Vector3 gripCenter = objectPose.Position + gripRot;

            return gripCenter;
        }

        /// <summary>
        /// Gets an estimate of the position of the horizontal "table" surface from the object pose
        /// This is a temporary approach to obstacle (surface) avoidance. It will be replaced by 
        /// a more comprehensive obstacle avoidance
        /// </summary>
        /// <param name="objectPose">Object pose</param>
        /// <returns>Distance to the horizontal surface</returns>
        public override double GetHorizontalPlaneOffset(Pose objectPose)
        {
            return objectPose.Position.Z - (this.ManipulatedObjectDescription.HeightInM / 2.0);
        }

        /// <summary>
        /// Gets a suggested gripper opening in meter for the grip style
        /// </summary>
        /// <param name="gripStyle">Grip Style</param>
        /// <returns>Gripper opening in M</returns>
        public override double GetGripperOpeningInM(GripStyle gripStyle)
        {
            double ret = this.ManipulatedObjectDescription.WidthInM;
            switch (gripStyle)
            {
                case GripStyle.TopDownAroundPrincipalAxis:
                    ret = this.ManipulatedObjectDescription.WidthInM;
                    break;

                case GripStyle.WrapAroundPrincipalAxisFace1:
                    ret = 0.0;
                    break;

                default:
                    ret = this.ManipulatedObjectDescription.WidthInM;
                    break;
            }

            return ret;
        }

        /// <summary>
        /// Description of the object
        /// </summary>
        /// <returns>String description</returns>
        public override string ToString()
        {
            return "Cylindrical can";
        }

        /// <summary>
        /// Returns palm plane configuration for wraparound grip
        /// </summary>
        /// <param name="objectPose">Object's pose</param>
        /// <returns>plane normal direction and distance from origin</returns>
        private Vector4 GetWrapAroundPalmPlane(Pose objectPose)
        {
            Vector3 pv = objectPose.Position;

            Vector3 tmpVec = new Vector3(pv);
            tmpVec.Z = 0.0;

            Quaternion rot = this.HeuristicGripPlaneRotationForWrapAround(GripStyle.WrapAroundPrincipalAxisFace1, objectPose);
            rot = rot * this.defaultOrientation;
            Vector3 n = Vector3.Normalize(Quaternion.Rotate(rot, tmpVec));

            Vector4 plane = this.PlaneDistancefromOrigin(n, pv);
            return plane;
        }

        /// <summary>
        /// Returns palm plane configuration for top pick grip
        /// </summary>
        /// <param name="objectPose">Object's pose</param>
        /// <returns>plane normal direction and distance from origin</returns>
        private Vector4 GetTopDownPalmPlane(Pose objectPose)
        {
            Vector3 defaultAxis = new Vector3(0, 0, -1);
            Vector3 n = Vector3.Normalize(Quaternion.Rotate(this.defaultOrientation, defaultAxis));

            Vector4 plane = this.PlaneDistancefromOrigin(n, this.GetGripCenter(GripStyle.TopDownAroundPrincipalAxis, objectPose));
            return plane;
        }

        /// <summary>
        /// When cylinder is vertical the wrist is inclined by a heuristic angle to make sure the arm wrist
        /// does not collide with the surface
        /// </summary>
        /// <param name="gripStyle">The grip style</param>
        /// <param name="objectPose">Object's pose in arm frame</param>
        /// <returns>Rule based orientation</returns>
        private Quaternion HeuristicGripPlaneRotationForWrapAround(GripStyle gripStyle, Pose objectPose)
        {
            Quaternion rotation = new Quaternion(0, 0, 0, 1);

            if (gripStyle == GripStyle.WrapAroundPrincipalAxisFace1)
            {
                Vector3 gripAxis = Quaternion.Rotate(this.defaultOrientation, new Vector3(0, 0, 1));
                double cosAng = Vector3.Dot(gripAxis, new Vector3(0, 0, 1));

                // Only apply for upright cans
                if (Math.Abs(cosAng - 1.0) < 0.01)
                {
                    Vector3 defaultNormal = new Vector3(objectPose.Position.X, objectPose.Position.Y, 0.0);
                    Vector3 rotAxis = Vector3.Normalize(Vector3.Cross(gripAxis, defaultNormal));
                    AxisAngle aa = new AxisAngle(rotAxis, this.wrapAroundHeuristicgripAngle);
                    rotation = Quaternion.FromAxisAngle(aa);
                }
            }

            return rotation;
        }
    }
}
